/*
 * Copyright (c) Huawei Technologies Co., Ltd. 2024-2024. All rights reserved.
 */

package com.huawei.codecraft;

import java.util.*;

/**
 * Main
 *
 * @since 2024-02-05
 * @describe 运行每秒50帧，每帧1000/50=20ms时间，15ms为决策时间
 */
public class Main {
    // 地图大小
    private static final int n = 200;
    // 机器人数量
    private static final int robot_num = 10;
    // 船数量
    private static final int boat_num = 5;
    // 泊位数量
    private static final int berth_num = 10;
    // 金钱，船容积，id
    private int money, boat_capacity, id;
    // 地图数据,200*200区域，左上角为原点，向右为y正轴，下为x正轴
    private char[][] map = new char[n][n];
    // 机器人实例，1*1大小，可携带一个物品。机器人位于泊位处可以放下物品，在货物处可以搬运物品。
    private Robot[] robot = new Robot[robot_num];
    // 泊位实例，4*4的矩形，(x, y) ——> (x+3, y+3).每个泊位有装载时间和产生收益时间。
    // 泊位信息一局中是固定的。初赛不区分泊位海岸和陆地部分。
    private Berth[] berth = new Berth[berth_num];
    // 船实例，2*4的矩形，初赛仅考虑泊位和运输时间
    private Boat[] boat = new Boat[boat_num];
    // 货物列表，随机生成货物。每个物品在指定位置停留20s(1000帧).
    private List<Good> goodList = new ArrayList<>();
    // 贵重货物列表
    private List<Good> importantGoodList = new ArrayList<>();
    // 路径数组
    private List<List<Node>> pathList = new ArrayList<>();
    // 泊位位置集合
    private Set<String> berthSet = new HashSet<>();
    // 用于机器人运送货物的5个泊位
    private Set<Integer> chooseBerthSet = new HashSet<>();
    // 初始化
    private void init() {
        Scanner scanf = new Scanner(System.in);
        // 读入地图数据, 从1开始
        for(int i = 0; i < n; i++) {
            String str = scanf.nextLine();
            for (int j = 0; j < n; j++) {
                map[i][j] =  str.charAt(j);
            }
        }
        // 读入10条泊位数据
        for (int i = 0; i < berth_num; i++) {
            int id = scanf.nextInt();
            berth[id] = new Berth();
            berth[id].id = id;
            berth[id].x = scanf.nextInt();
            berth[id].y = scanf.nextInt();
            berth[id].time = scanf.nextInt();
            berth[id].velocity = scanf.nextInt();
            berthSet.add(berth[id].x + "," + berth[id].y);
        }
        // 读入船的容积
        this.boat_capacity = scanf.nextInt();
        // 初始化机器人数据,10条
        for(int i = 0; i < robot_num; i++) {
            robot[i] = new Robot(); // 机器人实例化
            robot[i].mbx = -1;
            robot[i].mby = -1;
        }
        // 初始化船数据
        for(int i = 0; i < boat_num; i ++) {
            boat[i] = new Boat();   // 船实例化
        }
        // 初始化路径列表
        for(int i = 0; i < robot_num; i++) {
            pathList.add(new ArrayList<>());
        }

        // BidirectionalAStar search = new BidirectionalAStar(map);
        // int k = 0;
        // for (int i = 0; i < n; i++) {
        //     for (int j = 0; j < n; j++) {
        //         if (map[i][j] == 'A') {
        //             // 机器人
        //             robot[k].x = i;
        //             robot[k].y = j;
        //             k++;
        //         }
        //     }
        // }
        // // 根据地图、机器人、泊位数据进行泊位筛选，选择5个最优泊位，并保证每个机器人都有泊位能够到达
        // Berth[] sortBerthByDistance = sortBerthByDistance(berth);
        // List<List<Berth>> robotForBerth = new ArrayList<>();
        // for(int i = 0; i < robot_num; i++) {
        //     List<Berth> tempRobotForBerth = new ArrayList<>();
        //     for (int j = 0; j < berth_num; j++) {
        //         Node start = new Node(robot[i].x, robot[i].y, null, 0.0, 0.0);
        //         Node end = new Node(berth[j].x, berth[j].y, null, 0.0, 0.0);
        //         List<Node> path = search.findPath(start, end);
        //         // System.err.println("robot:"+i+" berth:"+j);
        //         if (path.size() > 0) {
        //             tempRobotForBerth.add(berth[j]);
        //         }
        //     }
        //     robotForBerth.add(tempRobotForBerth);
        // }
        // System.err.println("robotForBerth:"+ robotForBerth.size());
        // Set<Robot> robotSet = new HashSet<>();
        // Berth[] berthChoosed = new Berth[boat_num];
        // for (int i = 0; i < boat_num; i++) {
        //     for (int j = 0; j < robot_num; j++) {
        //         if (robotForBerth.get(j).contains(sortBerthByDistance[i])) {
        //             robotSet.add(robot[j]);
        //             berthChoosed[i] = sortBerthByDistance[i];
        //         }
        //     }
        // }
        // if (robotSet.size() < robot_num) {
        //     int robotNumDiff = robot_num - robotSet.size();
        //     for (int i = 0; i < berth_num; i++) {
        //         for (int j = 0; j < robot_num; j++) {
        //             if (!robotSet.contains(robot[j])) {
        //                 if (robotForBerth.get(j).contains(sortBerthByDistance[i])) {
        //                     robotSet.add(robot[j]);
        //                     berthChoosed[boat_num - robotNumDiff] = sortBerthByDistance[j];
        //                     robotNumDiff--;
        //                 }
        //             }
        //             if (robotNumDiff == 0) break;
        //         }
        //         if (robotNumDiff == 0) break;
        //     }
        // }
        // System.err.println(berthChoosed.length);
        // for (Berth value : berthChoosed) {
        //     System.err.println("选择的泊位id：" + value.id);
        //     chooseBerthSet.add(value.id);
        // }

        String okk = scanf.nextLine();
        System.out.println("OK");
        System.out.flush();
    }
    // 每一帧结算顺序
    // 1.机器人恢复
    // 2.船舶到达、进入港口
    // 3.物品生成
    // 4.生成场面信息，输出给选手
    // 5.读取选手指令
    // 6.执行机器人指令
    // 7.执行船舶指令
    // 8.港口装卸货物
    // 每一帧交互
    private int input() {
        Scanner scanf = new Scanner(System.in);
        // 帧序号和当前金钱数
        this.id = scanf.nextInt();
        this.money = scanf.nextInt();
        // 更新已有货物的存在时间
        // 判断物品是否过期
        this.goodList.removeIf(good -> 1000 - this.id + good.saveFrame <= 0);
        this.importantGoodList.removeIf(good -> 1000 - this.id + good.saveFrame <= 0);
        // 场上新增货物数量
        int num = scanf.nextInt();
        // 读入每个货物数据
        for (int i = 1; i <= num; i++) {
            int x = scanf.nextInt();    // 货物x坐标
            int y = scanf.nextInt();    // 货物y坐标
            int val = scanf.nextInt();  //货物的金额（<=1000)
            int saveFrame = id;
            Good good = new Good(x, y, val, saveFrame, -1);
            if (val > 120) {
                this.importantGoodList.add(good);    // 将货物信息装入列表
            } else {
                this.goodList.add(good);    // 将货物信息装入列表
            }
        }
        // 机器人数据,10条
        for(int i = 0; i < robot_num; i++) {
            // robot[i] = new Robot(); // 机器人实例化
            robot[i].id = i;   // 机器人编号
            robot[i].goods = scanf.nextInt();   // 是否携带货物，0未携带，1携带
            robot[i].x = scanf.nextInt();   // 机器人x坐标
            robot[i].y = scanf.nextInt();   // 机器人y坐标
            robot[i].status = scanf.nextInt();  // 机器人状态：0恢复状态，1正常状态
        }
        // 船数据
        for(int i = 0; i < boat_num; i ++) {
            // boat[i] = new Boat();   // 船实例化
            boat[i].status = scanf.nextInt();   // 船状态
            boat[i].pos = scanf.nextInt();  // 目标泊位ID，虚拟店为-1
        }
        // 将地图中机器人位置设置为A, 现将之前为A的元素设置为.
        for (int i = 0; i < n; i++) {
            for (int j = 0; j < n; j++) {
                if (map[i][j] == 'A') {
                    map[i][j] = '.';
                }
            }
        }
        // System.err.println("count = " + count);
        for(int i = 0; i < robot_num; i++) {
            map[robot[i].x][robot[i].y] = 'A';
        }
        String okk = scanf.nextLine();
        return id;
    }

    public static void main(String[] args) {
        Main mainInstance = new Main();
        // 初始化
        mainInstance.init();
        // 实例化路径查找算法
        BidirectionalAStar search = new BidirectionalAStar(mainInstance.map);
        // 输出时每一帧交互
        for(int zhen = 1; zhen <= 15000; zhen ++) {
            int id = mainInstance.input();
            // 路径规划算法实例化
            // 使用A*算法
            // AStarSearch search = new AStarSearch(mainInstance.map);
            // 使用双向A*算法
            // BidirectionalAStar search = new BidirectionalAStar(mainInstance.map);
            // 先进行机器人的交互，按照"指令 机器人id 参数2"的格式输出
            // 机器人每帧移动一格，机器人如果碰壁或海、相撞，会原地停留20帧（400ms）
            // 机器人与物品坐标重合时，可以使用get获取该坐标物品。
            // 机器人与泊位重合时，可以使用pull命令将物品放下。
            // 每帧动作分为移动前动作、移动、移动后动作三部分
            // 如果发生碰撞，同一帧所有动作均失效，碰撞后回到原位
            // System.err.println("robot_num：" + robot_num);
            for(int i = 0; i < robot_num; i ++) {
                // todo 寻找机器人周围最近的货物，并给机器人赋值要去的坐标
                // System.lineSeparator()返回行分隔符，unix返回\n，windows返回\r\n
                // 机器人正常运行，不正常则不进行任何操作
                if (mainInstance.robot[i].status == 1) {
                    // 机器人携带了货物
                    if (mainInstance.robot[i].mbx != -1) {
                        if (mainInstance.robot[i].goods == 1) {
                            // 判断机器人与目标重合,泊位在矩形4*4范围内就执行
                            // System.err.println("机器人" + mainInstance.robot[i].id + " 当前位置: " + mainInstance.robot[i].x + " " + mainInstance.robot[i].y + " 目标位置: " + mainInstance.robot[i].mbx + " " + mainInstance.robot[i].mby);
                            if (mainInstance.robot[i].mbx <= mainInstance.robot[i].x &&
                                mainInstance.robot[i].x <= mainInstance.robot[i].mbx + 3 &&
                                mainInstance.robot[i].mby <= mainInstance.robot[i].y &&
                                mainInstance.robot[i].y <= mainInstance.robot[i].mby + 3) {

                                System.out.printf("pull %d" + System.lineSeparator(), i);
                                mainInstance.robot[i].goods = 0;
                                mainInstance.robot[i].mbx = -1;
                                mainInstance.robot[i].mby = -1;
                                mainInstance.berth[i].goodCount += 1;
                            }
                        } else {
                            // 机器人未携带货物
                            // 判断机器人与目标重合
                            if (mainInstance.robot[i].mbx == mainInstance.robot[i].x &&
                                mainInstance.robot[i].mby == mainInstance.robot[i].y) {

                                System.out.printf("get %d" + System.lineSeparator(), i);
                                mainInstance.robot[i].goods = 1;
                                mainInstance.robot[i].mbx = -1;
                                mainInstance.robot[i].mby = -1;
                            }
                        }
                    }

                    List<Node> path = new ArrayList<>();

                    // 判断机器人是否已经有目的地
                    if (mainInstance.robot[i].mbx != -1) {
                        // todo 机器人未携带物品去泊位
                        if (mainInstance.robot[i].goods == 0 && mainInstance.berthSet.contains(mainInstance.robot[i].mbx + "," + mainInstance.robot[i].mby)) {
                            mainInstance.robot[i].mbx = -1;
                            mainInstance.robot[i].mby = -1;
                        // 机器人带了物品，但是目标不是泊位
                        } else if (mainInstance.robot[i].goods == 1 && !mainInstance.berthSet.contains(mainInstance.robot[i].mbx + "," + mainInstance.robot[i].mby)) {
                            mainInstance.robot[i].mbx = -1;
                            mainInstance.robot[i].mby = -1;
                        // 机器人有目标且符合要求
                        } else {
                            path = mainInstance.pathList.get(i);
                            int nextX = path.get(1).x;
                            int nextY = path.get(1).y;
                            // 机器人下一步会撞上，则更新路径数组
                            if (mainInstance.map[nextX][nextY] == 'A') {
                                Node start = new Node(mainInstance.robot[i].x, mainInstance.robot[i].y, null, 0, 0);
                                Node end = new Node(mainInstance.robot[i].mbx, mainInstance.robot[i].mby, null, 0, 0);
                                path = search.findPath(start, end);
                                mainInstance.pathList.set(i, path);
                            }
                        }
                    }
                    if (mainInstance.robot[i].mbx == -1) {
                        // 未携带货物，获取到最近的货物的路径
                        if (mainInstance.robot[i].goods == 0) {
                            // path = findEarliestGoodPath(mainInstance.robot[i], mainInstance.goodList, mainInstance.importantGoodList, id, mainInstance.map);
                            path = findNearestGoodPath(mainInstance.robot[i], mainInstance.goodList, mainInstance.importantGoodList, id, mainInstance.map);
                        }
                        // 携带货物获取最近的泊位的路径
                        if (mainInstance.robot[i].goods == 1) {
                            path = findNearestBerthPath(mainInstance.robot[i], mainInstance.berth, id, mainInstance.map, mainInstance.chooseBerthSet);
                        }
                        // 将定制的路径存入路径数组
                        mainInstance.pathList.set(i, path);
                    }

                    // 如果有最短路径就进行移动
                    if (path.size() > 1) {
                        mainInstance.robot[i].mbx = path.get(path.size() - 1).x;
                        mainInstance.robot[i].mby = path.get(path.size() - 1).y;
                        int nextX = path.get(1).x;
                        int nextY = path.get(1).y;
                        // 将机器人的下一步设置为A，这样路径规划会跳过
                        mainInstance.map[nextX][nextY] = 'A';
                        // 获取机器人移动参数
                        int moveParam = getRobotMoveParam(mainInstance.robot[i].x, mainInstance.robot[i].y, nextX, nextY);
                        if (moveParam != -1) {
                            System.out.printf("move %d %d" + System.lineSeparator(), i, moveParam);
                            // 删除当前位置坐标
                            path.remove(0);
                            mainInstance.pathList.set(i, path);
                        }
                    } else {
                        mainInstance.robot[i].mbx = -1;
                        mainInstance.robot[i].mby = -1;
                    }
                }
            }

            // 进行船的交互，按照"指令 船id 参数2"的格式输出。依次执行到达、驶离和装载。
            // 轮船容积装满之后不能再装，装载顺序为机器人防止货物的顺序。
            // 两艘船如果同一帧到达同一泊位，按照指令先后顺序进入。
            // 不考虑船的路径规划情况下，泊位间移动速度都是500帧（10s）。
            // 此处要考虑装载时间和到虚拟店时间，在程序即将结束前即使没有装满也要将船开走，保证收益获取！！！
            for (int i = 0; i < boat_num; i++) {
                // 船为正常状态
                if (mainInstance.boat[i].status == 1) {
                    // 船为装货状态,目标泊位为泊位id，容积大于0
                    if (mainInstance.boat[i].pos != -1) {
                        // 船没装满,装一半就开走
                        if (mainInstance.boat[i].capacity >= 0 && mainInstance.boat[i].capacity < mainInstance.boat_capacity) {
                            // todo 如何判断装载速度，跳帧是否影响装载
                            // if (mainInstance.berth[mainInstance.boat[i].pos].goodCount >= mainInstance.berth[mainInstance.boat[i].pos].velocity) {
                            if (mainInstance.berth[mainInstance.boat[i].pos].goodCount >= 0) {
                                mainInstance.boat[i].capacity += mainInstance.berth[mainInstance.boat[i].pos].velocity;
                                // System.err.println("装载船：" + i + " 泊位货物总量：" + mainInstance.berth[mainInstance.boat[i].pos].goodCount + " 船容积：" + mainInstance.boat[i].capacity);
                                mainInstance.berth[mainInstance.boat[i].pos].goodCount -= mainInstance.berth[mainInstance.boat[i].pos].velocity;
                            } else {
                                int targetBearthId = getNearestBerthByBoat(id, mainInstance.berth, mainInstance.chooseBerthSet);
                                // 没找到其他港口就驶离
                                if (targetBearthId == -1) {
                                    System.err.println("1 装载船：" + i + " 驶离卖货 剩余时间" + (15000 - id) + " 前往时间：" + mainInstance.berth[mainInstance.boat[i].pos].time + " 船容积：" + mainInstance.boat[i].capacity + " 容积上限：" + mainInstance.boat_capacity);
                                    System.out.printf("go %d" + System.lineSeparator(), i);
                                    mainInstance.boat[i].capacity = 0;  // 船驶离后容积重置
                                    mainInstance.berth[mainInstance.boat[i].pos].isUsed = false;
                                } else {
                                    System.out.printf("ship %d %d" + System.lineSeparator(), i, targetBearthId);
                                    mainInstance.berth[mainInstance.boat[i].pos].isUsed = false;
                                    mainInstance.boat[i].pos = targetBearthId;
                                }
                            }
                            // 如果装满了或者剩余时长小于泊位到虚拟点时间，则驶离
                            if (mainInstance.boat[i].capacity >= mainInstance.boat_capacity || mainInstance.berth[mainInstance.boat[i].pos].time + 1 > 15000 - id) {
                                System.err.println("2 装载船：" + i + " 驶离卖货 剩余时间" + (15000 - id) + " 前往时间：" + mainInstance.berth[mainInstance.boat[i].pos].time + " 船容积：" + mainInstance.boat[i].capacity + " 容积上限：" + mainInstance.boat_capacity);
                                System.out.printf("go %d" + System.lineSeparator(), i);
                                mainInstance.boat[i].capacity = 0;  // 船驶离后容积重置
                                mainInstance.berth[mainInstance.boat[i].pos].isUsed = false;
                            }
                        } else {
                            System.err.println("3 装载船：" + i + " 驶离卖货 剩余时间" + (15000 - id) + " 前往时间：" + mainInstance.berth[mainInstance.boat[i].pos].time + " 船容积：" + mainInstance.boat[i].capacity + " 容积上限：" + mainInstance.boat_capacity);
                            System.out.printf("go %d" + System.lineSeparator(), i);
                            mainInstance.boat[i].capacity = 0;  // 船驶离后容积重置
                            mainInstance.berth[mainInstance.boat[i].pos].isUsed = false;
                        }
                    } else {
                        // 如果船目标泊位为虚拟点，则选择最新且有货物没船去的泊位
                        int targetBearthId = getNearestBerth(mainInstance.berth, mainInstance.chooseBerthSet);
                        // int targetBearthId = getMostGoodBerth(mainInstance.berth);
                        mainInstance.boat[i].pos = targetBearthId;
                        System.out.printf("ship %d %d" + System.lineSeparator(), i, targetBearthId);
                    }
                }
                // todo 如果船在泊点外等待，则选择最近且有货物的泊位
                if (mainInstance.boat[i].status == 2) {
                    int targetBearthId = getNearestBerthByBoat(id, mainInstance.berth, mainInstance.chooseBerthSet);
                    mainInstance.boat[i].pos = targetBearthId;
                    System.out.printf("ship %d %d" + System.lineSeparator(), i, targetBearthId);
                }
            }
            System.out.println("OK");
            System.out.flush();
        }
    }
    // 机器人类
    static class Robot {
        int id; // 机器人id
        int x, y, goods;    // 机器人坐标和是否携带货物
        int status; // 机器人状态，0为恢复，1为正常
        int mbx, mby;   // 机器人目标坐标
        int goodValue; // 机器人携带货物价值

        public Robot() {}

        public Robot(int startX, int startY) {
            this.x = startX;
            this.y = startY;
        }

        public Robot(int id, int x, int y, int status, int mbx, int mby, int goodValue) {
            this.id = id;
            this.x = x;
            this.y = y;
            this.status = status;
            this.mbx = mbx;
            this.mby = mby;
            this.goodValue = goodValue;
        }
    }
    // 泊位类
    static class Berth {
        int id; // 泊位id
        int x;  // 泊位x坐标
        int y;  // 泊位y坐标
        int time;   // 泊位运输时间（到虚拟点）
        int velocity;   // 泊位装载速度（每帧装载数量）
        boolean isUsed; // 是否在使用中
        int goodCount;  // 泊位当前货物价值
        public Berth(){}
        public Berth(int id, int x, int y, int time, int velocity, int goodCount) {
            this.id = id;
            this.x = x;
            this.y = y;
            this.time = time;
            this.velocity = velocity;
            this.goodCount = goodCount;
        }
    }
    // 船类
    static class Boat {
        int status; // 运输状态,0为运输中，1为正常状态
        int pos;    // 目标泊位id，虚拟点为-1
        int capacity;   // 船的容积
    }
    // 货物类
    static class Good {
        int x;  // 货物x坐标
        int y;  // 货物y坐标
        int value;  // 货物价值
        int saveFrame;   // 货物生产帧数
        int robotId;   // 机器人id

        public Good(int x, int y, int val, int saveFrame, int robotId) {
            this.x = x;
            this.y = y;
            this.value = val;
            this.saveFrame = saveFrame;
            this.robotId = robotId;
        }
    }

    // 根据当前机器人对货物排序
    static class GoodDistanceComparator implements Comparator<Good> {
        private final Robot robot;

        public GoodDistanceComparator(Robot robot) {
            this.robot = robot;
        }

        @Override
        public int compare(Good g1, Good g2) {
            // double distance1 = Math.sqrt(Math.pow(robot.x - g1.x, 2) + Math.pow(robot.y - g1.y, 2));
            // double distance2 = Math.sqrt(Math.pow(robot.x - g2.x, 2) + Math.pow(robot.y - g2.y, 2));
            // 使用曼哈顿距离
            double distance1 = Math.abs(robot.x - g1.x) + Math.abs(robot.y - g1.y);
            double distance2 = Math.abs(robot.x - g2.x) + Math.abs(robot.y - g2.y);
            return Double.compare(distance1, distance2);
        }
    }

    // 查找机器人到所有货物的最近距离，返回货物的下标
    private static List<Node> findNearestGoodPath(Robot robot, List<Good> goods, List<Good> importantGoodList, int currentFrame, char[][] map) {
        long startTime = System.currentTimeMillis();
        List<Node> path = new ArrayList<>();
        // 使用A*算法
        // AStarSearch search = new AStarSearch(map);
        // 使用双向A*算法
        BidirectionalAStar search = new BidirectionalAStar(map);
        // 对每个货物进行排序，得到距离从近到远的货物列表
        // 优先贵重货物
        importantGoodList.sort(new GoodDistanceComparator(robot));
        // for (Good good : importantGoodList) {
        //     System.err.println("dist： " + Math.sqrt(Math.pow(robot.x - good.x, 2) + Math.pow(robot.y - good.y, 2)));
        // }
        for (int i = 0; i < importantGoodList.size(); i++) {
            // 方案二：寻找时间最早的物品，避免超时
            // 如果1.5倍直线距离都小，就不用规划路径了
            // int dist = (int) Math.sqrt(Math.pow(robot.x - importantGoodList.get(i).x, 2) + Math.pow(robot.y - importantGoodList.get(i).y, 2));
            // 使用曼哈顿
            double dist = Math.abs(robot.x - importantGoodList.get(i).x) + Math.abs(robot.y - importantGoodList.get(i).y);
            if ((1000 - currentFrame + importantGoodList.get(i).saveFrame) <= dist || dist >= 100.) {
                continue;
            }
            Node start = new Node(robot.x, robot.y, null, 0, 0);
            Node end = new Node(importantGoodList.get(i).x, importantGoodList.get(i).y, null, 0, 0);
            List<Node> tempPath = search.findPath(start, end);
            // 机器人能到达才去取货物
            if (tempPath.size() < (1000 - currentFrame + importantGoodList.get(i).saveFrame) && tempPath.size() > 0) {
                path = tempPath;
                robot.goodValue = importantGoodList.get(i).value;
                importantGoodList.remove(i);
                long endTime = System.currentTimeMillis();
                System.err.println("机器人：" + robot.id + " 规划路径，耗时：" + (endTime - startTime) + "ms" + " 规划路径长度：" + path.size());
                return path;
            }
        }
        // 如果没有贵重货物达到要求就取普通货物
        goods.sort(new GoodDistanceComparator(robot));
        for (int i = 0; i < goods.size(); i++) {
            // 方案二：寻找时间最早的物品，避免超时
            // 如果1.5倍直线距离都小，就不用规划路径了
            int dist = (int) Math.sqrt(Math.pow(robot.x - goods.get(i).x, 2) + Math.pow(robot.y - goods.get(i).y, 2));
            if ((1000 - currentFrame + goods.get(i).saveFrame) <= dist) {
                continue;
            }
            Node start = new Node(robot.x, robot.y, null, 0, 0);
            Node end = new Node(goods.get(i).x, goods.get(i).y, null, 0, 0);
            List<Node> tempPath = search.findPath(start, end);
            // 机器人能到达才去取货物
            if (tempPath.size() <= (1000 - currentFrame + goods.get(i).saveFrame) && tempPath.size() > 0) {
                path = tempPath;
                robot.goodValue = goods.get(i).value;
                goods.remove(i);
                long endTime = System.currentTimeMillis();
                System.err.println("机器人：" + robot.id + " 规划路径，耗时：" + (endTime - startTime) + "ms" + " 规划路径长度：" + path.size());
                return path;
            }
        }
        long endTime = System.currentTimeMillis();
        System.err.println("机器人：" + robot.id + " 规划路径，耗时：" + (endTime - startTime) + "ms" + " 未找到指定路径");
        return path;
    }

    // 查找机器人到最早的货物，返回货物的下标
    private static List<Node> findEarliestGoodPath(Robot robot, List<Good> goods, List<Good> importantGoodList, int currentFrame, char[][] map) {
        List<Node> path = new ArrayList<>();
        // 使用A*算法
        // AStarSearch search = new AStarSearch(map);
        // 使用双向A*算法
        BidirectionalAStar search = new BidirectionalAStar(map);
        // 对每个货物进行排序，得到距离从近到远的货物列表
        // 优先贵重货物
        importantGoodList.sort(new GoodDistanceComparator(robot));
        // System.err.println("importantGoodList.size(): " + importantGoodList.size());
        // for (Good good : importantGoodList) {
        //     System.err.println("goodvalue: " + good.value);
        // }
        for (int i = 0; i < importantGoodList.size(); i++) {
            // 方案二：寻找时间最早的物品，避免超时
            // 如果1.5倍直线距离都小，就不用规划路径了
            double dist = (int) Math.sqrt(Math.pow(robot.x - importantGoodList.get(i).x, 2) + Math.pow(robot.y - importantGoodList.get(i).y, 2));
            if ((1000 - currentFrame + importantGoodList.get(i).saveFrame) <= dist || dist >= 100) {
                continue;
            }
            Node start = new Node(robot.x, robot.y, null, 0, 0);
            Node end = new Node(importantGoodList.get(i).x, importantGoodList.get(i).y, null, 0, 0);
            List<Node> tempPath = search.findPath(start, end);
            // 机器人能到达才去取货物
            if (tempPath.size() <= (1000 - currentFrame + importantGoodList.get(i).saveFrame) && tempPath.size() > 0) {
                path = tempPath;
                robot.goodValue = importantGoodList.get(i).value;
                importantGoodList.remove(i);
                return path;
            }
        }
        // 如果没有贵重货物达到要求就取普通货物
        goods.sort(new GoodDistanceComparator(robot));
        for (int i = 0; i < goods.size(); i++) {
            // 方案二：寻找时间最早的物品，避免超时
            // 如果1.5倍直线距离都小，就不用规划路径了
            int dist = (int) Math.sqrt(Math.pow(robot.x - goods.get(i).x, 2) + Math.pow(robot.y - goods.get(i).y, 2));
            if ((1000 - currentFrame + goods.get(i).saveFrame) <= dist) {
                continue;
            }
            Node start = new Node(robot.x, robot.y, null, 0, 0);
            Node end = new Node(goods.get(i).x, goods.get(i).y, null, 0, 0);
            List<Node> tempPath = search.findPath(start, end);
            // 机器人能到达才去取货物
            if (tempPath.size() <= (1000 - currentFrame + goods.get(i).saveFrame) && tempPath.size() > 0) {
                path = tempPath;
                robot.goodValue = goods.get(i).value;
                goods.remove(i);
                return path;
            }
        }
        return path;
    }

    // 根据当前机器人对泊位排序
    private static Berth[] berthSorted(Robot robot, Berth[] berths) {
        Berth[] sortedBerths = berths.clone();
        Arrays.sort(sortedBerths, (o1, o2) -> {
            double distance1 = Math.sqrt(Math.pow(robot.x - o1.x, 2) + Math.pow(robot.y - o1.y, 2));
            double distance2 = Math.sqrt(Math.pow(robot.x - o2.x, 2) + Math.pow(robot.y - o2.y, 2));
            return Double.compare(distance1, distance2);
        });
        return sortedBerths;
    }

    // 查找机器人到所有泊位的最近距离，返回货物的下标
    private static List<Node> findNearestBerthPath(Robot robot, Berth[] berths, int currentFrame, char[][] map, Set<Integer> chooseBerthSet) {
        List<Node> path = new ArrayList<>();
        Berth[] berthSortedArray = berthSorted(robot, berths);
        // 使用A*算法
        // AStarSearch search = new AStarSearch(map);
        // 使用双向A*算法
        BidirectionalAStar search = new BidirectionalAStar(map);
        for (Berth berthItem : berthSortedArray) {
            if (berthItem.isUsed || !chooseBerthSet.contains(berthItem.id)) {
                continue;
            }
            // long startTime = System.currentTimeMillis();
            // System.err.println("机器人：" + robot.id + " 泊位使用情况:" + berthItem.isUsed);
            int dist = (int) Math.sqrt(Math.pow(robot.x - berthItem.x, 2) + Math.pow(robot.y - berthItem.y, 2));
            if (dist < 15000 - currentFrame) {
                Node start = new Node(robot.x, robot.y, null, 0, 0);
                // 定位到泊位中间
                Node end = new Node(berthItem.x, berthItem.y, null, 0, 0);
                List<Node> tempPath = search.findPath(start, end);
                // System.err.println("到泊位路径长度:" + tempPath.size() + " 泊位位置：" + berthItem.x + "," + berthItem.y + " 机器人位置：" + robot.x + "," + robot.y);
                // 泊位有船才去
                if (tempPath.size() > 0) {
                    path = tempPath;
                    return path;
                }
            }
            // long endTime = System.currentTimeMillis();
            // long duration = (endTime - startTime);
            // System.err.println("机器人：" + robot.id + " 泊位:" + berthItem.id + " 使用情况耗时:" + duration);
        }

        // 没有泊位可去，则随机移动
        Node start = new Node(robot.x, robot.y, null, 0, 0);
        Node end = new Node(berthSortedArray[0].x, berthSortedArray[0].y, null, 0, 0);
        List<Node> tempPath = search.findPath(start, end);
        if (tempPath.size() > 0) {
            path = tempPath;
            return path;
        }
        return path;
    }

    // 根据前后坐标获取机器人移动参数
    private static int getRobotMoveParam(int currentX, int currentY, int nextX, int nextY) {
        // 向右
        if (currentX == nextX && currentY < nextY) {
            return 0;
        }
        // 向左
        if (currentX == nextX && currentY > nextY) {
            return 1;
        }
        // 向上
        if (currentX > nextX && currentY == nextY) {
            return 2;
        }
        // 向下
        if (currentX < nextX && currentY == nextY) {
            return 3;
        }
        return -1;
    }

    // 将泊位按照货物存放总价值排序,从大到小
    private static Berth[] sortBerthByValue(Berth[] berths) {
        Berth[] sortedBerths = berths.clone();
        Arrays.sort(sortedBerths, (o1, o2) -> {
            int value1 = o1.goodCount;
            int value2 = o2.goodCount;
            return Integer.compare(value2, value1);
        });
        return sortedBerths;
    }

    // 获取最近有最多货物且没船的泊位
    private static int getMostGoodBerth(Berth[] berths, Set<Integer> chooseBerthSet) {
        int maxBerthIndex = -1;
        int maxGoodCount = 0;
        // 循环每个泊位
        for (Berth item : berths) {
            if (!chooseBerthSet.contains(item.id) || item.isUsed) {
                continue;
            }
            int goodCount = item.goodCount;
            // 如果存在更短时间的泊位，则判断该泊位是否存在船只
            if (maxGoodCount < goodCount) {
                maxBerthIndex = item.id;
                maxGoodCount = goodCount;
            }
        }
        if (maxBerthIndex == -1) {
            maxBerthIndex = getNearestBerth(berths, chooseBerthSet);
        }
        return maxBerthIndex;
    }

    // 将泊位按照距离虚拟点远近排序
    private static Berth[] sortBerthByDistance(Berth[] berths) {
        Berth[] sortedBerths = berths.clone();
        Arrays.sort(sortedBerths, (o1, o2) -> {
            int distance1 = o1.time;
            int distance2 = o2.time;
            return Integer.compare(distance1, distance2);
        });
        return sortedBerths;
    }

    // 获取最近有货物且没船去的泊位，没有货物则选择最近的泊位去
    private static int getNearestBerth(Berth[] berths, Set<Integer> chooseBerthSet) {
        int minBerthIndex = -1;
        int minTime = Integer.MAX_VALUE;
        // 循环每个泊位
        for (Berth item : berths) {
            // if (!chooseBerthSet.contains(item.id) || item.isUsed) {
            //     continue;
            // }
            if (item.isUsed) {
                continue;
            }
            // 获取泊位时间
            int time = item.time;
            // 如果存在更短时间的泊位，则判断该泊位是否存在船只
            if (minTime > time) {
                minTime = time;
                minBerthIndex = item.id;
            }
        }
        if (minBerthIndex != -1) {
            berths[minBerthIndex].isUsed = true;
        }
        return minBerthIndex;
    }

    // 获取当前船到另一个最近泊位，且保证船最后能到虚拟点
    private static int getNearestBerthByBoat(int frame, Berth[] berths, Set<Integer> chooseBerthSet) {
        int minBerthIndex = -1;
        int minTime = Integer.MAX_VALUE;
        int berthToBerthTime = 500; // 泊位之间移动需要500帧
        // 循环每个泊位
        for (Berth item : berths) {
            // if (!chooseBerthSet.contains(item.id) || item.isUsed) {
            //     continue;
            // }
            if (item.isUsed) {
                continue;
            }
            // 获取泊位时间
            int time = item.time;
            // System.err.println("time:" + time + "剩余帧数：" + (15000 - frame));
            // 如果存在更短时间的泊位，则判断该泊位是否存在船只
            if (minTime > time && (time + berthToBerthTime < 15000 - frame) && item.goodCount > 0){
                minTime = time;
                minBerthIndex = item.id;
            }
        }
        if (minBerthIndex != -1) {
            berths[minBerthIndex].isUsed = true;
        }
        return minBerthIndex;
    }

    // 根据船的id指定泊位，每隔2个指定一个
    private static int getBerthByBoat(int boatId, Berth[] berths, int frame) {
        int minBerthIndex = -1;
        int minTime = Integer.MAX_VALUE;
        int berthToBerthTime = 500; // 泊位之间移动需要500帧
        // 循环每个泊位
        for (int i = 0; i < berths.length; i++) {
            // 获取泊位时间
            int time = berths[i].time;
            // System.err.println("time:" + time + "剩余帧数：" + (15000 - frame));
            // 如果存在更短时间的泊位，则判断该泊位是否存在船只
            if (minTime > time && !berths[i].isUsed && (time + berthToBerthTime < 15000 - frame) && berths[i].goodCount > 0 && i % 2 == 0){
                minTime = time;
                minBerthIndex = berths[i].id;
            }
        }
        if (minBerthIndex != -1) {
            berths[minBerthIndex].isUsed = true;
        }
        return minBerthIndex;
    }
    // 路径规划算法实现
    // RRT_CONNECT采样算法
    // private int[] rrtConnect() {
    //
    // }
    // Dijkstra算法，基于当前机器人位置进行搜索，寻找最近的目标货物，计算到达时间
    // class Dijkstra() {
    //
    // }
    // A*算法，基于当前机器人搜索道最近目标点的最短路径
    // 节点类
    static class Node {
        int x, y;   // 节点坐标
        double g;   // 起点到该点的距离
        double h;   // 该点到终点的估计距离
        double f;   // f = g + h
        Node parent;    // 父节点

        public Node(int x, int y, Node parent, double g, double h) {
            this.x = x;
            this.y = y;
            this.parent = parent;
            this.g = g;
            this.h = h;
            this.f = g + h;
        }
    }

    public static class AStarSearch {
        private final int[][] DIRECTIONS = {{-1, 0}, {1, 0}, {0, -1}, {0, 1}};   // 上下左右移动
        private final char EMPTY = '.';
        private final char BERTH = 'B';
        private final char OBSTACLE = '#';
        private final char START = 'A';
        private final char GOAL = 'G';
        private char[][] map;
        private int width, height;

        public AStarSearch(char[][] map) {
            this.map = map;
            this.width = map[0].length;
            this.height = map.length;
        }

        // 欧式距离作为启发式函数
        private double heuristic(Node a, Node b) {
            return Math.sqrt(Math.pow(b.x - a.x, 2) + Math.pow(b.y - a.y, 2));
        }
        // 曼哈顿距离作为启发式函数
        private double manhattanHeuristic(Node a, Node b) {
            return Math.abs(b.x - a.x) + Math.abs(b.y - a.y);
        }
        // 判断坐标是否有效
        private boolean isValid(int x, int y) {
            return x >= 0 && x < height && y >= 0 && y < width && (map[x][y] == EMPTY || map[x][y] == BERTH);
        }
        // todo 在进行路径规划时，需要考虑下一步是否也在机器人下一步的坐标中，在的话就跳过
        // 获取邻近节点
        private List<Node> getNeighbors(Node node) {
            List<Node> neighbors = new ArrayList<>();
            for (int[] direction : DIRECTIONS) {
                int newX = node.x + direction[0];
                int newY = node.y + direction[1];
                if (isValid(newX, newY)) {
                    double tentativeGScore = node.g + 1;    // 移动一格距离增加1
                    Node neighbor = new Node(newX, newY, node, tentativeGScore, manhattanHeuristic(node, new Node(newX, newY, null, 0, 0)));
                    neighbors.add(neighbor);
                }
            }
            return neighbors;
        }
        // 寻找最优路径
        public List<Node> findPath(Node start, Node goal) {
            Set<String> visited = new HashSet<>();  //
            PriorityQueue<Node> openSet = new PriorityQueue<>(Comparator.comparingDouble(a -> a.f));  // 优先队列按照f值排序
            openSet.add(start);

            while (!openSet.isEmpty()) {
                Node current = openSet.poll();
                if (current.x == goal.x && current.y == goal.y) {
                    // 找到目标节点，返回路径
                    List<Node> path = new ArrayList<>();
                    while (current != null) {
                        path.add(current);
                        current = current.parent;
                    }
                    Collections.reverse(path);  // 回溯列表需要进行反转
                    return path;
                }

                visited.add(current.x + "," + current.y);  // 将当前节点加入已访问集合

                for (Node neighbor : getNeighbors(current)) {
                    // 如果邻居节点未被访问过且不在开放集合中，则将其加入开放集合
                    if (!visited.contains(neighbor.x + "," + neighbor.y) && openSet.stream().noneMatch(n -> n.x == neighbor.x && n.y == neighbor.y)) {
                        openSet.add(neighbor);  // 将邻居节点加入开放集合
                    }
                }
            }

            return new ArrayList<>();  // 如果没有找到路径，返回null
        }
    }
    // 双向A*算法，基于机器人和货物点位进行搜索，寻找最优路径。
    public static class BidirectionalAStar {
        private final int[][] DIRECTIONS = {{-1, 0}, {1, 0}, {0, -1}, {0, 1}};   // 上下左右移动
        private final char EMPTY = '.';
        private final char BERTH = 'B';
        private final char OBSTACLE = '#';
        private final char START = 'A';
        private final char GOAL = 'G';
        private char[][] map;
        private int width, height;

        public BidirectionalAStar(char[][] map) {
            this.map = map;
            this.width = map[0].length;
            this.height = map.length;
        }

        // 欧式距离作为启发式函数
        private double heuristic(Node a, Node b) {
            return Math.sqrt(Math.pow(b.x - a.x, 2) + Math.pow(b.y - a.y, 2));
        }
        // 曼哈顿距离作为启发式函数
        private double manhattanHeuristic(Node a, Node b) {
            return Math.abs(b.x - a.x) + Math.abs(b.y - a.y);
        }
        // 判断坐标是否有效
        private boolean isValid(int x, int y) {
            return x >= 0 && x < height && y >= 0 && y < width && (map[x][y] == EMPTY || map[x][y] == BERTH);
        }

        public List<Node> findPath(Node start, Node goal) {
            // System.err.println("Start: " + start.x + "," + start.y + ", Goal: " + goal.x + "," + goal.y);
            PriorityQueue<Node> openSetStart = new PriorityQueue<>(Comparator.comparingDouble(a -> a.f));
            PriorityQueue<Node> openSetEnd = new PriorityQueue<>(Comparator.comparingDouble(a -> a.f));
            Set<String> visitedSetStart = new HashSet<>();
            Set<String> visitedSetEnd = new HashSet<>();
            Map<String, Node> parentMapStart = new HashMap<>();
            Map<String, Node> parentMapEnd = new HashMap<>();

            openSetStart.add(start);
            openSetEnd.add(goal);

            Node meetPoint = null;
            while (!openSetStart.isEmpty() && !openSetEnd.isEmpty()) {
                Node currentStart = openSetStart.poll();
                Node currentEnd = openSetEnd.poll();

                visitedSetStart.add(currentStart.x + "," + currentStart.y);
                visitedSetEnd.add(currentEnd.x + "," + currentEnd.y);

                for (Node neighbor : getNeighbors(currentStart)) {
                    if (!visitedSetStart.contains(neighbor.x + "," + neighbor.y) && openSetStart.stream().noneMatch(n -> n.x == neighbor.x && n.y == neighbor.y)) {
                        parentMapStart.put(neighbor.x + "," + neighbor.y, currentStart);
                        openSetStart.add(neighbor);
                        if (visitedSetEnd.contains(neighbor.x + "," + neighbor.y)) {
                            meetPoint = neighbor;
                            return reconstructPath(meetPoint, parentMapEnd.get(meetPoint.x + "," + meetPoint.y));
                        }
                    }
                }

                for (Node neighbor : getNeighbors(currentEnd)) {
                    if (!visitedSetEnd.contains(neighbor.x + "," + neighbor.y) && openSetEnd.stream().noneMatch(n -> n.x == neighbor.x && n.y == neighbor.y)) {
                        parentMapEnd.put(neighbor.x + "," + neighbor.y, currentEnd);
                        openSetEnd.add(neighbor);
                        if (visitedSetStart.contains(neighbor.x + "," + neighbor.y)) {
                            meetPoint = neighbor;
                            return reconstructPath(parentMapStart.get(meetPoint.x + "," + meetPoint.y), meetPoint);
                        }
                    }
                }
            }
            return new ArrayList<>();
        }


        private static List<Node> reconstructPath(Node forward, Node backward) {
            List<Node> path = new ArrayList<>();
            Node current = forward;
            while (current != null) {
                path.add(current);
                current = current.parent;
            }
            Collections.reverse(path);
            current = backward;
            while (current != null) {
                path.add(current);
                current = current.parent;
            }

            return path;
        }

        private List<Node> getNeighbors(Node node) {
            // 根据实际情况获取邻居节点
            List<Node> neighbors = new ArrayList<>();
            for (int[] direction : DIRECTIONS) {
                int newX = node.x + direction[0];
                int newY = node.y + direction[1];
                if (isValid(newX, newY)) {
                    double tentativeGScore = node.g + 1;    // 移动一格距离增加1
                    Node neighbor = new Node(newX, newY, node, tentativeGScore, manhattanHeuristic(node, new Node(newX, newY, null, 0, 0)));
                    neighbors.add(neighbor);
                }
            }
            return neighbors;
        }
    }

    // todo 蚁群算法
    static class AntColonySystem {
        // 实现蚁群算法


    }

}
